#include <Arduino.h>
#include <FS.h>
#include <SPIFFS.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include "encoder.h"
#include "motor.h"
#include "ws2812.h"
#include <math.h>
#include "mpu6050.h"
#include "comunicate.h"
#include "ros_process.h"
#include "config.h"
#include "param.h"

UDPComunicate udp;
Param param;
WheelEncoder enc1(Encoder1_A, Encoder1_B, 1);
WheelEncoder enc3(Encoder3_A, Encoder3_B, 1);
WheelEncoder enc2(Encoder2_A, Encoder2_B);
WheelEncoder enc4(Encoder4_A, Encoder4_B);
void data_read(void *arg);
void pid_task(void *arg);
xTaskHandle pid_task_handle;
xTaskHandle data_read_handle;
void setup()
{
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("start init");
  motor_init();
  udp.begin(8888);

  ws2812_init();
  delay(50);
  // ws2812_set_color(120, 0,120);
  ros_init(WIFI_REMOTE_IP, 11411);
  delay(30);
  param.begin_server();
  param.recover_param();
  BaseType_t xReturned;
  xReturned = xTaskCreatePinnedToCore(data_read, "data_read", DATA_READ_TASK_STACK_SIZE, NULL, DATA_READ_TASK_PRIORITY, &data_read_handle, 0);
  if (xReturned == pdPASS)
  {
    Serial.println("data_read task created");
  }
  else
  {
    Serial.printf("data_read task created failed, code 0x%lx\n", xReturned);
  }
  xReturned = xTaskCreatePinnedToCore(pid_task, "pid_task", PID_TASK_STACK_SIZE, NULL, PID_TASK_PRIORITY, &pid_task_handle, 0);
  if (xReturned == pdPASS)
  {
    Serial.println("pid_task task created");
  }
  else
  {
    Serial.printf("pid_task task created failed, code 0x%lx\n", xReturned);
  }
  // //start scheduler
  // vTaskStartScheduler();
  // Serial.println("start scheduler failed");
}

void pid_task(void *arg)
{
  short Motor_speed[4];
  SemaphoreHandle_t xMutex_pid;
  xMutex_pid = xSemaphoreCreateMutex();

  while (1)
  {

    // Serial.pr8520intf("out1:%d out2:%d out3:%d out4:%d\n", Motor_speed[0], Motor_speed[1], Motor_speed[2], Motor_speed[3]);
    xSemaphoreTake(xMutex_pid, portMAX_DELAY);
    gyro_pid.Compute(ros_imu_data.gz);
    speed_pid1.setpoint= ros_expect_speed_data.exps1  + gyro_pid.output;
    speed_pid2.setpoint= ros_expect_speed_data.exps2  - gyro_pid.output;
    speed_pid3.setpoint= ros_expect_speed_data.exps3  + gyro_pid.output;
    speed_pid4.setpoint= ros_expect_speed_data.exps4  - gyro_pid.output;
    speed_pid1.Compute(ros_speed_data.speed1);
    speed_pid2.Compute(ros_speed_data.speed2);
    speed_pid3.Compute(ros_speed_data.speed3);
    speed_pid4.Compute(ros_speed_data.speed4);
    Motor_speed[0] = speed_pid1.output;
    Motor_speed[1] = speed_pid2.output;
    Motor_speed[2] = speed_pid3.output;
    Motor_speed[3] = speed_pid4.output;
    xSemaphoreGive(xMutex_pid);

    xQueueSend(motor_q, Motor_speed, 0);
    String str;
    switch (flag1)
    {
    case 1:
      str = String(speed_pid1.setpoint) + "," + String(speed_pid1.input);
      udp.write(str);
      break;
    case 2:
      str = String(speed_pid2.setpoint) + "," + String(speed_pid2.input);
      udp.write(str);
      break;
    case 3:

      str = String(speed_pid3.setpoint) + "," + String(speed_pid3.input);
      udp.write(str);
      break;
    case 4:
      str = String(speed_pid4.setpoint) + "," + String(speed_pid4.input);
      udp.write(str);
      break;
    case 5:
      str = String(gyro_pid.setpoint) + "," + String(gyro_pid.input);
      udp.write(str);
      break;
    case 6:
      str = String(ros_imu_data.gz)+","+String(gyro_pid.input) +","+String(gyro_pid.setpoint) +","+String(gyro_pid.output);
      udp.write(str);
      break;

    default:
      break;
    };
    vTaskDelay(4);
  }
}
void data_read(void *arg)
{
  mpu_init();
  enc1.init_hardware();
  enc2.init_hardware();
  enc3.init_hardware();
  enc4.init_hardware();
  Serial.println("data task start");

  int idx = 0;
  while (1)
  {
    ros_distance_data.distance1 = enc1.get_distance();
    ros_distance_data.distance2 = enc2.get_distance();
    ros_distance_data.distance3 = enc3.get_distance();
    ros_distance_data.distance4 = enc4.get_distance();
    ros_speed_data.speed1 = enc1.get_speed();
    ros_speed_data.speed2 = enc2.get_speed();
    ros_speed_data.speed3 = enc3.get_speed();
    ros_speed_data.speed4 = enc4.get_speed();
    get_acc_gyro_anlge_kf(ros_imu_data.gx, ros_imu_data.gy,
                          ros_imu_data.gz, ros_imu_data.ax, ros_imu_data.ay, ros_imu_data.az);

    vTaskDelay(8 / portTICK_PERIOD_MS);
  }
}
void loop()
{
  // delay(1000);
  // put your main code here, to run repeatedly:
  // ws2812_set_color(120, 0,120);
  // delay(100);
  //  Serial.println(analogRead(1)*15.42257462/4096.0);
  // idx++;
  // Serial.printf("enc1: %d, enc2: %d, enc3: %d, enc4: %d\n", enc1.get_cnt(), enc2.get_cnt(), enc3.get_cnt(),
  // enc4.get_cnt());

  // Serial.printf("speed1: %f\tspeed2: %f\tspeed3: %f\tspeed4: %f\n", enc1.get_speed(), enc2.get_speed(),
  // enc3.get_speed(), enc4.get_speed()); Serial.printf("d1: %f\td2: %f\td3: %f\td4: %f\n", enc1.get_distance(),
  // enc2.get_distance(), enc3.get_distance(), enc4.get_distance());
  //   mpu_test(0);
  // print_acc_kf();
  // ws2812_set_color(120, 0, 120);
}
